#ifndef BTL_VIEWGEOMETRY_POINTTRIANGULATION
#define BTL_VIEWGEOMETRY_POINTTRIANGULATION

#include <btl/ViewGeometry/EssentialMatrix.hpp>
#include <Eigen/Core>

namespace btl
{
namespace viewgeometry
{

/// Triangulates a point as a homogenous vector from a point correspondence and an essential matrix.
///
/// Implemented according to Nister, 2004
///   An Efficient Solution to the Five-Point Relative Pose Problem
///   (appendix C)
///
/// @param  P   Relative pose
/// @param  E   The essential matrix, which must be build-able from the pose
/// @param  x1  Measurement 1 (a ray)
/// @param  x2  Measurement 2 (a ray)
/// @return X, for which (0 = x2^T * E * x1), where x1 and x2 are the measurements of X
Eigen::Vector4d triangulate(
      const Eigen::Isometry3d& P, const Eigen::Matrix3d& E,
      const Eigen::Vector3d& x1, const Eigen::Vector3d& x2);

Eigen::Vector4d triangulate(
      const Eigen::Isometry3d& P, const Eigen::Vector3d& x1, const Eigen::Vector3d& x2);


} // namespace viewgeometry
} // namespace btl

namespace btl
{

using viewgeometry::triangulate;

} // namespace btl

// ====================================================================
// === Implementation

namespace btl
{
namespace viewgeometry
{

inline Eigen::Vector4d triangulate(
      const Eigen::Isometry3d& P, const Eigen::Matrix3d& E,
      const Eigen::Vector3d& x1, const Eigen::Vector3d& x2)
{
   Eigen::Vector3d Eq = E * x1;
   Eigen::Vector3d c = x2.cross(Eigen::Vector3d(Eq(0), Eq(1), 0.0));
   Eigen::Vector4d C = P.matrix().topRows<3>().transpose() * c;

   Eigen::Vector4d x
      = Eigen::Vector4d(
         x1(0)*C(3),
         x1(1)*C(3),
         x1(2)*C(3),
         -(x1(0)*C(0) + x1(1)*C(1) + x1(2)*C(2))
      );

   // flip the sign if necessary so that only z needs to be tested to
   // determine whether the point is in front or behind the camera
   if (x(3) < 0.0)
      x *= -1.0;

   return x;
}

inline Eigen::Vector4d triangulate(
      const Eigen::Isometry3d& P, const Eigen::Vector3d& x1, const Eigen::Vector3d& x2)
{
   return triangulate(
      P, btl::viewgeometry::BuildEssentialMatrix(P).matrix(), x1, x2);
}

} // namespace viewgeometry
} // namespace btl

#endif // BTL_VIEWGEOMETRY_POINTTRIANGULATION
